extends SpeedStateResource


class_name TrackingStateResource
"""
追踪状态
"""


## 追踪目标分组
@export var group: String = ""
## 转向时同方向的夹角设置(夹角范围内认为是同方向,会开始加速)
@export_range(0, 180, 1, "radians") var turn_limited_angle: float = deg_to_rad(30)
## 转向时的最大限速
@export var turn_limited_speed: float = 0
## 距离目标点的停止距离(误差)
@export var stop_radius: float = 5


enum State {
    Idle,
    Tracking,
    Aiming,
    SpeedUp,
    SpeedDown,
    Arrive,
}


# 返回加速/减速的距离
func calc_speed_distance(v: float, v_to: float, a: float) -> float:
    return abs(v * v - v_to * v_to) / (2 * a)

# 返回当前速度停止后的移动距离
func get_speed_down_distance(v: float, a: float) -> float:
    return calc_speed_distance(0, v, a)

# 返回当前距离下从0开始先加速后减速的速度阀值
func get_limited_speed_from_distance(v: float, distance: float) -> float:
    # 加速已经走过的距离
    var speed_up_distance: float = get_speed_down_distance(v, acceleration)
    # 从0加速又减速到0的实际距离
    var move_distance: float = speed_up_distance + distance

    var a: float = acceleration
    var f: float = frictional
    var squared_v: float = (move_distance * 2 * a * f) / (a + f)
    return sqrt(squared_v)

# 直线上减速到指定速度,返回是否需要继续迭代
func speed_down_to(ctrl: MoveController, v: float, delta: float) -> bool:
    var speed: float = speed_down(ctrl, delta)
    ctrl.move_component.speed = speed
    return speed > v

# 直线上加速到指定速度,返回是否需要继续迭代
func speed_up_to(ctrl: MoveController, v: float, delta: float) -> bool:
    var speed: float = speed_up(ctrl, delta)
    ctrl.move_component.speed = speed
    return speed < v

# 加速或减速到指定速度
func speed_to(ctrl: MoveController, v: float, delta: float) -> bool:
    return speed_up_to(ctrl, v, delta) if ctrl.move_component.speed < v else speed_down_to(ctrl, v, delta)

# 是否到达
func is_arrive_position(ctrl: MoveController, data: Dictionary, point: Vector2) -> bool:
    # 不需要检测
    if data.arrive_check_tick > data.update_tick:
        return false
    var squared_dest_distance: float = data.destination.distance_squared_to(point)
    # 以最大速度移动的距离
    data.arrive_check_tick = data.update_tick + sqrt(squared_dest_distance) / max_speed
    return squared_dest_distance < data.squared_stop_raidus

# 曲线上移动(转向+调整速度),返回是否还在转向
func move_on_curve(ctrl: MoveController, data: Dictionary, move_direction: Vector2, delta: float) -> bool:
    const faraway_angle = 0.5 * PI

    if data.turn_check_tick < data.update_tick:
        var angle: float = abs(move_direction.angle_to(ctrl.move_component.direction))
        # 在加速限制角度内或者已经近似直线
        if angle < turn_limited_angle or angle < palstance * delta:
            return false
        # 还在转向中,判断是远离还是靠近目标点
        data.is_faraway_angle = (angle > faraway_angle)
        # 有角速度则缓存转向检测时间(提高性能)
        if palstance > 0:
            var cached_angle: float = (angle - faraway_angle) if data.is_faraway_angle else angle
            data.turn_check_tick = data.update_tick + (cached_angle / palstance)
    # 超速需要等减速到限速后再开始转向
    if ctrl.move_component.speed > turn_limited_speed:
        speed_down_to(ctrl, turn_limited_speed, delta)
        return true
    # 转向
    ctrl.move_component.direction = turn_to(ctrl, move_direction, delta)
    # 远离的角度一定需要减速(防止绕圈)
    if data.is_faraway_angle:
        ctrl.move_component.speed = speed_down(ctrl, delta)
    return true

func yield_speed(ctrl: MoveController, data: Dictionary, delta: float) -> void:
    var stand_point: Vector2 = get_stand_position(ctrl)
    var to_direction: Vector2 = (data.destination - stand_point).normalized()
    data.update_tick += delta
    # 在加速减速的过程中继续转向
    if turn_limited_angle > 0:
        ctrl.move_component.direction = turn_to(ctrl, to_direction, delta)

func yield_aiming(ctrl: MoveController, data: Dictionary, delta: float) -> int:
    var stand_point: Vector2 = get_stand_position(ctrl)
    var to_direction: Vector2 = (data.destination - stand_point).normalized()
    # 对准方向后判断是减速还是加速
    data.update_tick += delta
    # 已经到达目标点
    if ctrl.move_component.speed < data.min_limited_speed:
        if is_arrive_position(ctrl, data, stand_point):
            ctrl.move_component.speed = 0
            ctrl.move_component.wrapper.global_position = stand_point
            return State.Arrive
    # 先调转方向为正对目标
    if move_on_curve(ctrl, data, to_direction, delta):
        return State.Aiming
    # 当前速度下判断刹车距离
    var speed_down_distance: float = get_speed_down_distance(ctrl.move_component.speed, frictional)
    var dest_distance: float = (data.destination - stand_point).length()
    # 刹车距离不够,不再需要加速,直接减速
    if abs(speed_down_distance - dest_distance) < data.min_limited_speed:
        data.limited_speed = data.min_limited_speed
        return State.SpeedDown
    # 还可以加速,计算加速的速度阈值
    data.limited_speed = get_limited_speed_from_distance(ctrl.move_component.speed, dest_distance)
    # 不能超过限速
    data.limited_speed = min(max_speed, floorf(data.limited_speed))
    return State.SpeedUp

func yield_state(ctrl: MoveController, data: Dictionary, state: int, delta: float, duration: float) -> int:
    match state:
        State.Idle:
            return State.Tracking
        State.Arrive:
            return State.Idle
        State.Tracking:
            if ctrl.target == null:
                return State.Arrive
            data.destination = ctrl.target.global_position
            # 如果未开始移动,则设置初始方向
            if ctrl.move_component.is_stop():
                ctrl.move_component.direction = (data.destination - get_stand_position(ctrl)).normalized()
            data.limited_speed = 0
            data.turn_check_tick = 0
            data.arrive_check_tick = 0
            data.update_tick = 0
            data.squared_stop_raidus = stop_radius * stop_radius
            data.min_limited_speed = frictional * delta
            return State.Aiming
        State.Aiming:
            return yield_aiming(ctrl, data, delta)
        State.SpeedUp:
            yield_speed(ctrl, data, delta)
            # 加速
            if speed_up_to(ctrl, data.limited_speed, delta):
                return state
            return State.Aiming
        State.SpeedDown:
            yield_speed(ctrl, data, delta)
            # 减速
            if speed_down_to(ctrl, data.limited_speed, delta):
                return state
            return State.Aiming
    return state

func enter(ctrl: MoveController, data: Dictionary) -> void:
    data.state = State.Idle
    # 指定距离,从静止到加速再到静止时的最大速度
    data.limited_speed = 0.0
    # udpate计时器
    data.update_tick = 0.0
    # 下一次转向检测的时间
    data.turn_check_tick = 0.0
    # 下一次到达检测的时间
    data.arrive_check_tick = 0.0
    # 是否是远离的角度
    data.is_faraway_angle = false
    # 目标点
    data.destination = Vector2.ZERO
    # 缓存数据,停止距离的平方
    data.squared_stop_raidus = 0.0
    # 缓存数据,最小的限速
    data.min_limited_speed = 0.0

func update(ctrl: MoveController, data: Dictionary, delta: float, duration: float) -> bool:
    var state: State = yield_state(ctrl, data, data.state, delta, duration)
    if state == State.Arrive:
        return true
    data.state = state
    return false
